#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

int main(int argc, char **argv)
{
    // 通过命令行参数获取iris_id，默认为 iris_0
    std::string iris_id = "iris_0";
    if (argc > 1) 
    {
        // 启动节点时命令行输入参数会保存到argv中，此时将输入的无人机ID读取出来
        iris_id = argv[1];
    }

    ros::init(argc, argv, "offboard_arm_init" + iris_id);
    ros::NodeHandle nh;

    // 输出当前读取出的无人机ID
    ROS_INFO("iris_id: %s", iris_id.c_str());

    // 无人机解锁服务
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            (iris_id + "/mavros/cmd/arming");

    // 无人机切换飞行模式服务
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            (iris_id + "/mavros/set_mode");

    // 设置节点休眠周期
    ros::Rate rate(20.0);

    // 定义飞行模式对象并将飞行模式设为 offboard
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    // 定义解锁命令对象并将其设为 解锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    //这里其实可以不用加最外层的循环，但为了保证代码统一性就没进行删除，其实并不会进行循环
    while(ros::ok())
    {
        // 切换飞行模式
        set_mode_client.call(offb_set_mode);
        // 等待服务响应
        while (offb_set_mode.response.mode_sent == false && ros::ok()) 
        {
            ros::spinOnce();
            rate.sleep();
        }
        // 输出 模式切换成功
        ROS_INFO("Offboard enabled");

        // 解锁
        arming_client.call(arm_cmd);
        // 等待服务响应
        while (arm_cmd.response.success == false && ros::ok())
        {
            ros::spinOnce();
            rate.sleep();
        }
        // 输出 解锁成功
        ROS_INFO("Vehicle armed");
        // 跳出循环
        break;
    }

    return 0;
}
